#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
using namespace cv;

int WIDTH = 640;
int HEIGHT = 480;
int THRESH = 1;
double WHITE = 100;
double ALPHA = 0.05;

int main(int argc, char **argv) {
	VideoCapture camera(0); // video source for webcam
	camera.set(CV_CAP_PROP_FRAME_WIDTH,WIDTH);
	camera.set(CV_CAP_PROP_FRAME_HEIGHT,HEIGHT);
	// slices matrcies that hold H,S and V
	vector<Mat> slices;
	// create matrices to hold image
	Mat camImage;		// raw image from webcam
	Mat background;		// background image
	//Mat foreground;		// background image
	Mat diff;
	Mat final;

	camera >> camImage;
	camImage.copyTo(background);
	//camImage.copyTo(foreground);

	SimpleBlobDetector::Params params;
	params.minThreshold = 40;
	params.maxThreshold = 60;
	params.thresholdStep = 5;
	params.minDistBetweenBlobs = 1000;
	params.minArea = 500;
	//params.minConvexity = .4f;
	//params.minInertiaRatio = .1f;
	params.maxArea = 1000000;
	//params.maxConvexity = 2;
	params.filterByColor = false;
	params.filterByCircularity = false;
	params.filterByInertia = false;
	params.filterByConvexity = false;
	SimpleBlobDetector blobDetector( params );
	blobDetector.create("SimpleBlob");
	
	vector<KeyPoint> permKeyPoints;
	vector<KeyPoint> keyPoints;

	for(;;) {
		camera >> camImage;
		absdiff(background, camImage, diff);
		//threshold(diff, foreground, THRESH, WHITE, CV_THRESH_BINARY);

		background = (1 - ALPHA) * background  + ALPHA * camImage;

		camImage.copyTo(final);

		blobDetector.detect(diff, keyPoints);

		for(int i=0; i<keyPoints.size(); i++) {
			circle(final, keyPoints[i].pt, 20, cvScalar(255,255,255), 10);
			permKeyPoints.push_back(keyPoints[i]);
		}

		for (int i = 0; i < permKeyPoints.size() - 1; i++) {
			line(final, permKeyPoints[i].pt, permKeyPoints[i + 1].pt, CV_RGB(255,0,0));
		}

		imshow("Unedited", camImage);
		imshow("Background", background);
		//imshow("Foreground", foreground);
		imshow("Raw Diff", diff);
		imshow("Blob Tracking", final);

		if( cvWaitKey(10) >= 0 )
			break;
	}
	cvDestroyAllWindows();
}